#include "LFOSCommon.h"
#include "LineExtraction.h"
#include<vector>
#include<cstring>
#include<iostream>
#include<fstream>
#include<algorithm>


using namespace std;

bool PeakCompare(Peak p1, Peak p2)
{
	return (p1.accumVal > p2.accumVal);
}

LineSegment* LineExtraction::ExtractLines(bool* frame, unsigned int& numLineSegments, Point* edgePoints, int numPoints)
{
	int r, rindx, numLines;
	int thetas[MAXLINES];
	int rs[MAXLINES];
	Line lines[MAXLINES];
	Point p;
	int x, y;
	//fstream output;
	//output.open("houghout.txt", ios::out);

	//initialize accumulator to all zeros
//	Accumulator = {{0}};
//	for(int t=0; t < THETASIZE; t++)
//		for(int r=0; r < RSIZE; r++)
//			Accumulator[t][r] = 0;
	memset(Accumulator, 0, sizeof Accumulator);
	for(int i = 0; i < PIXELS; ++i)
		visited[i] = false;


//	for(int y = 0; y < ROWS; ++y)
//		for(int x = 0; x < COLS; ++x)
	for(int i = 0; i < numPoints; ++i)
		{
			x = edgePoints[i].x;
			y = edgePoints[i].y;
			if(frame[ROW_COL_TO_INDEX(x,y)])
			{
				for(int t=0; t < THETASIZE; t += 2)
				{
					r = (y*sin[t] + x*cos[t])/TRIGFACTOR;
					rindx = (r + NUMR/2)/RSTEP;
				    p.x = x;
				    p.y = y;
				    if(Accumulator[t/2][rindx] == 0)
				    {
				    	p.x = x;
						p.y = y;
						firstPoint[t/2][rindx] = p;
				    }
					++Accumulator[t/2][rindx];
					//output << t << "   " << rindx << "   x: " << x
					//		<< "   y:" << y << endl;
				}
			}
		}

	//output.close();

	//fprintf(stderr, "Finding Peaks\n");
	numLines = FindPeaks(thetas, rs);

	for(int i = 0; i < numLines; ++i)
	{
		lines[i].theta = thetas[i];
		lines[i].r = rs[i];
	}

	//fprintf(stderr, "Finding Segments\n");
	numLineSegments = FindSegments(lines, numLines, frame);

	return FoundLineSegments;
}

//Finds the local maxima in the accumulator array
//returns the number of peaks found and sets the theta and
//r values for each peak in the arrays
int LineExtraction::FindPeaks(int thetas[], int rs[])
{
	int numPeaks = 0;
	int t;
//	fstream output;
//	output.open("peaks.txt", ios::out);
	Peak pk;
	vector<Peak> peaks (500,pk);
	peaks.clear();
	make_heap(peaks.begin(), peaks.end(), PeakCompare);

	for(t = 0; t < THETASIZE/2 && numPeaks < MAXLINES; t++)
		for(int r = 0; r < RSIZE && numPeaks < MAXLINES; r++)
		{
//			cout << "t:" << t << endl;
//			cout << "r:" << r << endl;
			//allow theta to wrap for checking maximum
			if( r  > 0 && r < (RSIZE -1))
			{
				if (Accumulator[t][r] > PEAKTHRESHOLD &&
					Accumulator[t][r] > Accumulator[(t+THETASIZE-1) % THETASIZE][r] &&
					Accumulator[t][r] > Accumulator[(t+1) % THETASIZE][r] &&
					Accumulator[t][r] > Accumulator[t][r - 1] &&
					Accumulator[t][r] > Accumulator[t][r + 1])
				{
					//output << Accumulator[t][r] << "   " << t << "   " << r << "   theta: " << t*THETASTEP + THETAMIN
					//		<< "   r: " << r*RSTEP - NUMR/2 << endl;
					//thetas[numPeaks] = t*THETASTEP + THETAMIN;
					//rs[numPeaks] = r*RSTEP - NUMR/2;
//					peaks[numPeaks].theta = t*THETASTEP + THETAMIN;
//					peaks[numPeaks].r = r*RSTEP - NUMR/2;
//					peaks[numPeaks].accumVal = Accumulator[t][r];
					numPeaks++;
					pk.theta = 2*t*THETASTEP + THETAMIN;
					pk.r = r*RSTEP - NUMR/2;
					pk.accumVal = Accumulator[t][r];
					peaks.push_back(pk);
					push_heap(peaks.begin(), peaks.end(), PeakCompare);
				}
			}
			else if(r == 0)
			{
				if (Accumulator[t][r] > PEAKTHRESHOLD &&
					Accumulator[t][r] > Accumulator[(t+THETASIZE-1) % THETASIZE][r] &&
					Accumulator[t][r] > Accumulator[(t+1) % THETASIZE][r] &&
					Accumulator[t][r] > Accumulator[t][r + 1])
				{
					//output << Accumulator[t][r] << "   " << t << "   " << r << "   theta: " << t*THETASTEP + THETAMIN
					//		<< "   r: " << r*RSTEP - NUMR/2 << endl;
//					peaks[numPeaks].theta = t*THETASTEP + THETAMIN;
//					peaks[numPeaks].r = r*RSTEP - NUMR/2;
//					peaks[numPeaks].accumVal = Accumulator[t][r];
					numPeaks++;
					pk.theta = 2*t*THETASTEP + THETAMIN;
					pk.r = r*RSTEP - NUMR/2;
					pk.accumVal = Accumulator[t][r];
					peaks.push_back(pk);
					push_heap(peaks.begin(), peaks.end(), PeakCompare);
				}
			}
			else if(r == (RSIZE -1))
			{
				if (Accumulator[t][r] > PEAKTHRESHOLD &&
					Accumulator[t][r] > Accumulator[(t+THETASIZE-1) % THETASIZE][r] &&
					Accumulator[t][r] > Accumulator[(t+1) % THETASIZE][r] &&
					Accumulator[t][r] > Accumulator[t][r - 1])
				{
					//output << Accumulator[t][r] << "   " << t << "   " << r << "   theta: " << t*THETASTEP + THETAMIN
					//		<< "   r: " << r*RSTEP - NUMR/2 << endl;
//					peaks[numPeaks].theta = t*THETASTEP + THETAMIN;
//					peaks[numPeaks].r = r*RSTEP - NUMR/2;
//					peaks[numPeaks].accumVal = Accumulator[t][r];
					numPeaks++;
					pk.theta = 2*t*THETASTEP + THETAMIN;
					pk.r = r*RSTEP - NUMR/2;
					pk.accumVal = Accumulator[t][r];
					peaks.push_back(pk);
					push_heap(peaks.begin(), peaks.end(), PeakCompare);
				}
			}
		}

//	fprintf(stderr, "Sorting Peaks\n");
//	fprintf(stderr, "Num Peaks: %d\n", numPeaks);
	//sort(peaks, peaks+numPeaks, PeakCompare);
//	fprintf(stderr, "Finished sorting peaks\n");
	sort_heap(peaks.begin(), peaks.end(), PeakCompare);
	for(int i = 0; i < numPeaks; ++i)
	{
//		cout << peaks[i].accumVal << "   " << (peaks[i].theta - THETAMIN)/THETASTEP << "   " <<
//				(peaks[i].r + NUMR/2)/RSTEP << "   theta: "  << peaks[i].theta <<
//				"   r: " << peaks[i].r <<endl;
		thetas[i] = peaks[i].theta;
		rs[i] = peaks[i].r;
	}
//
//	output.close();

	return numPeaks;
}

//Creates Line Segments from the given lines
//returns the number of segments
int LineExtraction::FindSegments(Line lines[], unsigned int numLines, bool* frame)
{
	int numSegments = 0;
	LineSegment segment;
	bool isEdge;
	int t, r;
	int x, y;
	int theta;
	int error;
	Point p, first;
	//bool visited[PIXELS];

//	fprintf(stderr, "Num Lines: %d\n", numLines);
	for(unsigned int i = 0; i < numLines; ++i)
	{
		theta = lines[i].theta;
		r = lines[i].r;
		segment.theta = theta;
		segment.r = r;
		//fprintf(stderr, "Getting First Point\n");
		//fprintf(stderr, "theta: %d\n", theta);
		//fprintf(stderr, "t = %d, r = %d\n", (theta - THETAMIN)/THETASTEP, (r+ NUMR/2)/RSTEP);
		first = firstPoint[(theta - THETAMIN)/(2*THETASTEP)][(r + NUMR/2)/RSTEP];
		//fprintf(stderr, "Looping through lines");
		if(theta == 0 && r > 0 && r < (COLS - 1))
		{
			for(y = first.y; y < ROWS; ++y)
			{
				isEdge = false;
				//for(int j = -1; j <= 1; ++j)
				for(int j = 0; j <= 1; ++j)
				{
					if(frame[ROW_COL_TO_INDEX(r+j,y)])
					{
						isEdge = true;
						error = j;
						break;
					}
					if(frame[ROW_COL_TO_INDEX(r-j,y)])
					{
						isEdge = true;
						error = -j;
						break;
					}
				}

				if(isEdge && !visited[ROW_COL_TO_INDEX(r+error,y)])
				{
					p.x = r+error;
					p.y = y;
					numSegments += TraceLine(frame, r, theta, p, error, numSegments);
				}
			}
		}

		else if (theta >= -45 && theta <=45)
		{
			t = (theta - THETAMIN)/THETASTEP;
			for(y = first.y; y < ROWS; ++y)
			{
				x = (r*TRIGFACTOR)/cos[t] - (y*sin[t])/cos[t];
				if(x > 2 && x < COLS - 3)
				{
					isEdge = false;
					//for(int j = -3; j <= 3; ++j)
					for(int j = 0; j <= 3; ++j)
					{
						if(frame[ROW_COL_TO_INDEX(x+j,y)])
						{
							isEdge = true;
							error = j;
							break;
						}
						if(frame[ROW_COL_TO_INDEX(x-j,y)])
						{
							isEdge = true;
							error = -j;
							break;
						}
					}
					if(isEdge && !visited[ROW_COL_TO_INDEX(x+error,y)])
					{
						p.x = x+error;
						p.y = y;
						numSegments += TraceLine(frame, r, theta, p, error, numSegments);
					}


				}
			}// end for loop
		}
		else
		{
			t = (theta - THETAMIN)/THETASTEP;
			for(x = first.x; x < COLS; ++x)
			{
				y = r*TRIGFACTOR/sin[t] - x*cos[t]/sin[t];
				if(y > 2 && y < ROWS - 3)
				{
					isEdge = false;
					//for(int j = -3; j <= 3; ++j)
					for(int j = 0; j <= 3; ++j)
					{
						if(frame[ROW_COL_TO_INDEX(x,y+j)])
						{
							isEdge = true;
							error = j;
							break;
						}
						if(frame[ROW_COL_TO_INDEX(x,y-j)])
						{
							isEdge = true;
							error = -j;
							break;
						}
					}
					if(isEdge && !visited[ROW_COL_TO_INDEX(x,y+error)])
					{
						p.x = x;
						p.y = y + error;
						numSegments += TraceLine(frame, r, theta, p, error, numSegments);
					}
				}
			}// end for loop
		}
	}

	return numSegments;
}

int LineExtraction::TraceLine(bool* frame, int r, int theta, Point start, int error, int numSegments)
{
	bool isEdge, forwardSuccess;
	int x, y;
	int startError;
	Point last = start;
	LineSegment segment;
	int m, t;
	Point tempVisited[500];
	int visitedCount = 0;

	segment.theta = theta;
	segment.r = r;
	segment.p1.x = start.x;
	segment.p1.y = start.y;
	startError = error;
	forwardSuccess = false;

	if(theta == 0)
	{
		//start going forward
		for(y = start.y; y < ROWS; ++y)
		{
			isEdge = false;
			//for(int j = -1; j <= 1; ++j)
			for(int j = 0; j <= 1; ++j)
			{
				if((r+error+j) < COLS && (r+error+j) >= 0
						&& frame[ROW_COL_TO_INDEX(r+error+j,y)] &&
						!visited[ROW_COL_TO_INDEX(r+error+j,y)])
				{
					isEdge = true;
					error = error+j;
					break;
				}
				if((r+error-j) < COLS && (r+error-j) >= 0 &&
						frame[ROW_COL_TO_INDEX(r+error-j,y)] &&
						!visited[ROW_COL_TO_INDEX(r+error-j,y)])
				{
					isEdge = true;
					error = error-j;
					break;
				}
			}

			if(!isEdge && y - last.y > GAP)
			{
				if(last.y - segment.p1.y > 5)
				{
					segment.p2.x = last.x;
					segment.p2.y = last.y;
					forwardSuccess = true;
				}
				break;
			}
//			else if(!isEdge)
//			{
//				if(error < 0)
//					error--;
//				else if(error > 0)
//					error++;
//			}
			else if(isEdge && !visited[ROW_COL_TO_INDEX(r+error,y)])
			{
				last.x = r+error;
				last.y = y;
				tempVisited[visitedCount].x = r+error;
				tempVisited[visitedCount++].y = y;
			}
		}//end for loop
		//check in case line extends to bottom of frame
		if(!forwardSuccess && last.y - segment.p1.y > 5)
		{
			segment.p2.x = last.x;
			segment.p2.y = last.y;
			forwardSuccess = true;
		}
		//now go backwards
		if(forwardSuccess)
		{
			error = startError;
			last.x = start.x;
			last.y = start.y;

			for(y = start.y; y >= 0; y--)
			{
				isEdge = false;
				//for(int j = -1; j <= 1; ++j)
				for(int j = 0; j <= 1; ++j)
				{
					if((r+error+j) < COLS && (r+error+j) >= 0 &&
							frame[ROW_COL_TO_INDEX(r+error+j,y)] &&
							!visited[ROW_COL_TO_INDEX(r+error+j,y)])
					{
						isEdge = true;
						error = error+j;
						break;
					}
					if((r+error-j) < COLS && (r+error-j) >= 0 &&
							frame[ROW_COL_TO_INDEX(r+error-j,y)] &&
							!visited[ROW_COL_TO_INDEX(r+error-j,y)])
					{
						isEdge = true;
						error = error-j;
						break;
					}
				}

				if(!isEdge && last.y - y > GAP)
				{
					if(segment.p2.y - last.y > 15)
					{
						segment.p1.x = last.x;
						segment.p1.y = last.y;
						if(segment.p1.x == segment.p2.x)
							segment.theta = 0;
						else
						{
							m = -(segment.p1.y - segment.p2.y)*100/(segment.p1.x - segment.p2.x);
							segment.theta = cotLookup(m);
						}
						t = (segment.theta - THETAMIN)/THETASTEP;
						segment.r = (last.y*sin[t] + last.x*cos[t])/TRIGFACTOR;
						FoundLineSegments[numSegments] = segment;
						for(int i = 0; i < visitedCount; ++i)
						{
							visited[ROW_COL_TO_INDEX(tempVisited[i].x, tempVisited[i].y)] = true;
						}
						return 1;
					}
					break;
				}
//				else if(!isEdge)
//				{
//					if(error < 0)
//						error--;
//					else if(error > 0)
//						error++;
//				}
				else if(isEdge && !visited[ROW_COL_TO_INDEX(r+error,y)])
				{
					last.x = r+error;
					last.y = y;
					tempVisited[visitedCount].x = r+error;
					tempVisited[visitedCount++].y = y;
				}
			}
			if(segment.p2.y - last.y > 15)
			{
				segment.p1.x = last.x;
				segment.p1.y = last.y;
				if(segment.p1.x == segment.p2.x)
					segment.theta = 0;
				else
				{
					m = -(segment.p1.y - segment.p2.y)*100/(segment.p1.x - segment.p2.x);
					segment.theta = cotLookup(m);
				}
				t = (segment.theta - THETAMIN)/THETASTEP;
				segment.r = (last.y*sin[t] + last.x*cos[t])/TRIGFACTOR;
				FoundLineSegments[numSegments] = segment;
				for(int i = 0; i < visitedCount; ++i)
				{
					visited[ROW_COL_TO_INDEX(tempVisited[i].x, tempVisited[i].y)] = true;
				}
				return 1;
			}
		}
	}
	else if(theta >= -45 && theta <=45)
	{
		t = (theta - THETAMIN)/THETASTEP;
		//go forward first
		forwardSuccess = false;
		for(y = start.y; y < ROWS; ++y)
		{
			x = r*TRIGFACTOR/cos[t] - y*sin[t]/cos[t];
			if(x >=2 && x <= COLS - 3)
			{
				isEdge = false;
				//for(int j = -3; j <= 3; ++j)
				for(int j= 0; j <= ERRORCHECK; ++j)
				{
					if((x+error+j) < COLS && (x+error+j) >= 0 &&
							frame[ROW_COL_TO_INDEX(x+error+j,y)] &&
							!visited[ROW_COL_TO_INDEX(x+error+j,y)])
					{
						isEdge = true;
						error = error+j;
						break;
					}
					if((x+error-j) < COLS && (x+error-j) >= 0 &&
							frame[ROW_COL_TO_INDEX(x+error-j,y)] &&
							!visited[ROW_COL_TO_INDEX(x+error-j,y)])
					{
						isEdge = true;
						error = error-j;
						break;
					}
				}

				if(!isEdge && y - last.y > GAP)
				{
					if(last.y - segment.p1.y > 5)
					{
						segment.p2.x = last.x;
						segment.p2.y = last.y;
						forwardSuccess = true;
					}
					break;
				}
//				else if(!isEdge)
//				{
//					if(error < 0)
//						error--;
//					else if(error > 0)
//						error++;
//				}
				else if(isEdge && !visited[ROW_COL_TO_INDEX(x+error,y)])
				{
					last.x = x+error;
					last.y = y;
					tempVisited[visitedCount].x = x+error;
					tempVisited[visitedCount++].y = y;
				}
			}
		}//end for loop
		//check in case line extends to bottom of frame
		if(!forwardSuccess && last.y - segment.p1.y > 5)
		{
			segment.p2.x = last.x;
			segment.p2.y = last.y;
			forwardSuccess = true;
		}
		//now go backwards
		if(forwardSuccess)
		{
			error = startError;
			last.x = start.x;
			last.y = start.y;
			for(y = start.y-1; y >= 0; y--)
			{
				x = r*TRIGFACTOR/cos[t] - y*sin[t]/cos[t];
				if(x >=2 && x <= COLS - 3)
				{
					isEdge = false;
					//for(int j = -3; j <= 3; ++j)
					for(int j = 0; j <= ERRORCHECK; ++j)
					{
						if((x+error+j) < COLS && (x+error+j) >= 0 &&
								frame[ROW_COL_TO_INDEX(x+error+j,y)] &&
								!visited[ROW_COL_TO_INDEX(x+error+j,y)])
						{
							isEdge = true;
							error = error+j;
							break;
						}
						if((x+error+j) < COLS && (x+error+j) >= 0 &&
								frame[ROW_COL_TO_INDEX(x+error-j,y)] &&
								!visited[ROW_COL_TO_INDEX(x+error-j,y)])
						{
							isEdge = true;
							error = error-j;
							break;
						}
					}

					if(!isEdge && last.y - y > GAP)
					{
						if(segment.p2.y - last.y > 15)
						{
							segment.p1.x = last.x;
							segment.p1.y = last.y;
							if(segment.p1.x == segment.p2.x)
								segment.theta = 0;
							else
							{
								m = -(segment.p1.y - segment.p2.y)*100/(segment.p1.x - segment.p2.x);
								segment.theta = cotLookup(m);
							}
							t = (segment.theta - THETAMIN)/THETASTEP;
							segment.r = (last.y*sin[t] + last.x*cos[t])/TRIGFACTOR;
							FoundLineSegments[numSegments] = segment;
							for(int i = 0; i < visitedCount; i++)
							{
								visited[ROW_COL_TO_INDEX(tempVisited[i].x, tempVisited[i].y)] = true;
							}
							return 1;
						}
						break;
					}
//					else if(!isEdge)
//					{
//						if(error < 0)
//							error--;
//						else if(error > 0)
//							error++;
//					}
					else if(isEdge && !visited[ROW_COL_TO_INDEX(x+error,y)])
					{
						last.x = x+error;
						last.y = y;
						tempVisited[visitedCount].x = x+error;
						tempVisited[visitedCount++].y = y;
					}
				}
			}//end for loop
			if(segment.p2.y - last.y > 15)
			{
				segment.p1.x = last.x;
				segment.p1.y = last.y;
				if(segment.p1.x == segment.p2.x)
					segment.theta = 0;
				else
				{
					m = -(segment.p1.y - segment.p2.y)*100/(segment.p1.x - segment.p2.x);
					segment.theta = cotLookup(m);
				}
				t = (segment.theta - THETAMIN)/THETASTEP;
				segment.r = (last.y*sin[t] + last.x*cos[t])/TRIGFACTOR;
				FoundLineSegments[numSegments] = segment;
				for(int i = 0; i < visitedCount; ++i)
				{
					visited[ROW_COL_TO_INDEX(tempVisited[i].x, tempVisited[i].y)] = true;
				}
				return 1;
			}
		}
	}
	else
	{
		t = (theta - THETAMIN)/THETASTEP;
		//go forward first
		forwardSuccess = false;
		for(x = start.x; x < COLS; ++x)
		{
			y = r*TRIGFACTOR/sin[t] - x*cos[t]/sin[t];
			if(y >=2 && y <= COLS - 3)
			{
				isEdge = false;
				//for(int j = -3; j <= 3; ++j)
				for(int j = 0; j <= ERRORCHECK; ++j)
				{
					if((y+error+j) < ROWS && (y+error+j) >= 0 &&
							frame[ROW_COL_TO_INDEX(x,y+error+j)] &&
							!visited[ROW_COL_TO_INDEX(x,y+error+j)])
					{
						isEdge = true;
						error = error+j;
						break;
					}
					if((y+error-j) < ROWS && (y+error-j) >= 0 &&
							frame[ROW_COL_TO_INDEX(x,y+error-j)] &&
							!visited[ROW_COL_TO_INDEX(x,y+error-j)])
					{
						isEdge = true;
						error = error-j;
						break;
					}
				}

				if(!isEdge && x - last.x > GAP)
				{
					if(last.x - segment.p1.x > 5)
					{
						segment.p2.x = last.x;
						segment.p2.y = last.y;
						forwardSuccess = true;
					}
					break;
				}
//				else if(!isEdge)
//				{
//					if(error < 0)
//						error--;
//					else if(error > 0)
//						error++;
//				}
				else if(isEdge && !visited[ROW_COL_TO_INDEX(x,y+error)])
				{
					last.x = x;
					last.y = y+error;
					tempVisited[visitedCount].x = x;
					tempVisited[visitedCount++].y = y+error;
				}
			}
		}//end for loop
		//check in case line extends to bottom of frame
		if(!forwardSuccess && last.x - segment.p1.x > 5)
		{
			segment.p2.x = last.x;
			segment.p2.y = last.y;
			forwardSuccess = true;
		}
		//now go backwards
		if(forwardSuccess)
		{
			error = startError;
			last.x = start.x;
			last.y = start.y;
			for(x = start.x-1; x >= 0; x--)
			{
				y = r*TRIGFACTOR/sin[t] - x*cos[t]/sin[t];
				if(y >=2 && y <= ROWS - 3)
				{
					isEdge = false;
					//for(int j = -3; j <= 3; ++j)
					for(int j = 0; j <= ERRORCHECK; ++j)
					{
						if((y+error+j) < ROWS && (y+error+j) >= 0 &&
								frame[ROW_COL_TO_INDEX(x,y+error+j)] &&
								!visited[ROW_COL_TO_INDEX(x,y+error+j)])
						{
							isEdge = true;
							error = error+j;
							break;
						}
						if((y+error-j) < ROWS && (y+error+j) &&
								frame[ROW_COL_TO_INDEX(x,y+error-j)] &&
								!visited[ROW_COL_TO_INDEX(x,y+error-j)])
						{
							isEdge = true;
							error = error-j;
							break;
						}
					}

					if(!isEdge && last.x - x > GAP)
					{
						if(segment.p2.x - last.x > 15)
						{
							segment.p1.x = last.x;
							segment.p1.y = last.y;
							if(segment.p1.x == segment.p2.x)
								segment.theta = 0;
							else
							{
								m = -(segment.p1.y - segment.p2.y)*100/(segment.p1.x - segment.p2.x);
								segment.theta = cotLookup(m);
							}
							t = (segment.theta - THETAMIN)/THETASTEP;
							segment.r = (last.y*sin[t] + last.x*cos[t])/TRIGFACTOR;
							FoundLineSegments[numSegments] = segment;
							for(int i = 0; i < visitedCount; i++)
							{
								visited[ROW_COL_TO_INDEX(tempVisited[i].x, tempVisited[i].y)] = true;
							}
							return 1;
						}
						break;
					}
//					else if(!isEdge)
//					{
//						if(error < 0)
//							error--;
//						else if(error > 0)
//							error++;
//					}
					else if(isEdge && !visited[ROW_COL_TO_INDEX(x,y+error)])
					{
						last.x = x;
						last.y = y+error;
						tempVisited[visitedCount].x = x;
						tempVisited[visitedCount++].y = y+error;
					}
				}
			}//end for loop
			if(segment.p2.x - last.x > 15)
			{
				segment.p1.x = last.x;
				segment.p1.y = last.y;
				if(segment.p1.x == segment.p2.x)
					segment.theta = 0;
				else
				{
					m = -(segment.p1.y - segment.p2.y)*100/(segment.p1.x - segment.p2.x);
					segment.theta = cotLookup(m);
				}
				t = (segment.theta - THETAMIN)/THETASTEP;
				segment.r = (last.y*sin[t] + last.x*cos[t])/TRIGFACTOR;
				FoundLineSegments[numSegments] = segment;
				for(int i = 0; i < visitedCount; ++i)
				{
					visited[ROW_COL_TO_INDEX(tempVisited[i].x, tempVisited[i].y)] = true;
				}
				return 1;

			}
		}
	}
	return 0;
}

int LineExtraction::cotLookup(int m)
{
	if(m <= -5727)
		return 0;
	else if (m <= -1908)
		return -2;
	else if(m <= -1143)
		return -4;
	else if(m <= -815)
		return -6;
	else if(m <= -632)
		return -8;
	else if(m <= -515)
		return -10;
	else if(m <= -434)
		return -12;
	else if(m <= -374)
		return -14;
	else if(m <= -328)
		return -16;
	else if(m <= -291)
		return -18;
	else if(m <= -261)
		return -20;
	else if(m <= -236)
		return -22;
	else if(m <= -215)
		return -24;
	else if(m <= -197)
		return -26;
	else if(m <= -181)
		return -28;
	else if(m <= -167)
		return -30;
	else if(m <= -154)
		return -32;
	else if(m <= -143)
		return -34;
	else if(m <= -133)
		return -36;
	else if(m <= -124)
		return -38;
	else if(m <= -116)
		return -40;
	else if(m <= -107)
		return -42;
	else if(m <= -100)
		return -44;
	else if(m <= -94)
		return -46;
	else if(m <= -87)
		return -48;
	else if(m <= -81)
		return -50;
	else if(m <= -75)
		return -52;
	else if(m <= -70)
		return -54;
	else if(m <= -65)
		return -56;
	else if(m <= -60)
		return -58;
	else if(m <= -55)
		return -60;
	else if(m <= -51)
		return -62;
	else if(m <= -47)
		return -64;
	else if(m <= -43)
		return -66;
	else if(m <= -38)
		return -68;
	else if(m <= -34)
		return -70;
	else if(m <= -31)
		return -72;
	else if(m <= -27)
		return -74;
	else if(m <= -23)
		return -76;
	else if(m <= -19)
		return -78;
	else if(m <= -16)
		return -80;
	else if(m <= -12)
		return -82;
	else if(m <= -9)
		return -84;
	else if(m <= -5)
		return -86;
	else if(m <= -2)
		return -88;
	else if(m <= 2)
		return 90;
	else if(m <= 5)
		return 88;
	else if(m <= 9)
		return 86;
	else if(m <= 12)
		return 84;
	else if(m <= 16)
		return 82;
	else if(m <= 19)
		return 80;
	else if(m <= 23)
		return 78;
	else if(m <= 27)
		return 76;
	else if(m <= 31)
		return 74;
	else if(m <= 34)
		return 72;
	else if(m <= 38)
		return 70;
	else if(m <= 43)
		return 68;
	else if(m <= 47)
		return 66;
	else if(m <= 51)
		return 64;
	else if(m <= 55)
		return 62;
	else if(m <= 60)
		return 60;
	else if(m <= 65)
		return 58;
	else if(m <= 70)
		return 56;
	else if(m <= 75)
		return 54;
	else if(m <= 81)
		return 52;
	else if(m <= 87)
		return 50;
	else if(m <= 94)
		return 48;
	else if(m <= 100)
		return 46;
	else if(m <= 107)
		return 44;
	else if(m <= 116)
		return 42;
	else if(m <= 124)
		return 40;
	else if(m <= 133)
		return 38;
	else if(m <= 143)
		return 36;
	else if(m <= 154)
		return 34;
	else if(m <= 167)
		return 32;
	else if(m <= 181)
		return 30;
	else if(m <= 197)
		return 28;
	else if(m <= 215)
		return 26;
	else if(m <= 236)
		return 24;
	else if(m <= 261)
		return 22;
	else if(m <= 291)
		return 20;
	else if(m <= 328)
		return 18;
	else if(m <= 374)
		return 16;
	else if(m <= 434)
		return 14;
	else if(m <= 515)
		return 12;
	else if(m <= 632)
		return 10;
	else if(m <= 815)
		return 8;
	else if(m <= 1143)
		return 6;
	else if(m <= 1908)
		return 4;
	else if(m <= 5727)
		return 2;
	else
		return 0;

}

//short LineExtraction::cos[THETASIZE] = {87,174,259,342,423,500,574,643,707,766,819,866,906,940,966,985,996,1000,996,985,966,940,
//		906,866,819,766,707,643,574,500,423,342,259,174,87,0};
//
//
//short LineExtraction::sin[THETASIZE] = {-996,-985,-966,-940,-906,-866,-819,-766,-707,-643,-574,-500,-423,-342,-259,-174,-87,0,
//		87,174,259,342,423,500,574,643,707,766,819,866,906,940,966,985,996,1000};


short LineExtraction::cos[THETASIZE] = {35, 70, 105, 139, 174, 208, 242, 276, 309, 342, 375, 407, 438, 469, 500, 530, 559, 588,
		616, 643, 669, 695, 719, 743, 766, 788, 809, 829, 848, 866, 883, 899, 914, 927, 940, 951, 961, 970, 978, 985, 990, 995,
		998, 999, 1000, 999, 998, 995, 990, 985, 978, 970, 961, 951, 940, 927, 914, 899, 883, 866, 848, 829, 809, 788, 766, 743,
		719, 695, 669, 643, 616, 588, 559, 530, 500, 469, 438, 407, 375, 342, 309, 276, 242, 208, 174, 139, 105, 70, 35, 0};

short LineExtraction::sin[THETASIZE] = {-999, -998, -995, -990, -985, -978, -970, -961, -951, -940, -927, -914, -899, -883, -866,
		-848, -829, -809, -788, -766, -743, -719, -695, -669, -643, -616, -588, -559, -530, -500, -469, -438, -407, -375, -342, -309,
		-276, -242, -208, -174, -139, -105, -70, -35, 0, 35, 70, 105, 139, 174, 208, 242, 276, 309, 342, 375, 407, 438, 469, 500, 530,
		559, 588, 616, 643, 669, 695, 719, 743, 766, 788, 809, 829, 848, 866, 883, 899, 914, 927, 940, 951, 961, 970, 978, 985, 990,
		995, 998, 999, 1000};

//short LineExtraction::cos[THETASIZE] = {70, 139, 208, 276, 342, 407, 469, 530, 588, 643, 695, 743, 788, 829, 866, 899, 927, 951,
//		970, 985, 995, 999, 999, 995, 985, 970, 951, 927, 899, 866, 829, 788, 743, 695, 643, 588, 530, 469, 407, 342, 276, 208,
//		139, 70, 0};
//
//short LineExtraction::sin[THETASIZE] = {-998, -990, -978, -961, -940, -914, -883, -848, -809, -766, -719, -669, -616, -559, -500,
//		-438, -375, -309, -242, -174, -105, -35, 35, 105, 174, 242, 309, 375, 438, 500, 559, 616, 669, 719, 766, 809, 848, 883,
//		914, 940, 961, 978, 990, 998, 1000};
